/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 

#include <cob_vision_utils/VisionUtils.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <fstream>


using namespace ipa_Utils;

cv::Mat ipa_Utils::vstack(const std::vector<cv::Mat> &mats)
{
    if (mats.empty())
        return cv::Mat();

    // we need to know the total number of rows to create the stacked matrix
    int nRows = 0;
    int nCols = mats.front().cols;
    int datatype = mats.front().type();
    std::vector<cv::Mat>::const_iterator it;
    for (it = mats.begin(); it != mats.end(); ++it)
    {
        nRows += it->rows;
    }

    // copy data to stacked matrix
    int startRow = 0;
    int endRow = 0;
    cv::Mat stacked(nRows, nCols, datatype);
    for (it = mats.begin(); it != mats.end(); ++it)
    {
        if (it->rows == 0)
            continue;

        // make sure all mats have same num of cols and data type
        CV_Assert(it->cols == nCols);
        CV_Assert(it->type() == datatype);

        startRow = endRow;
        endRow = startRow + it->rows;
    	cv::Mat mat = stacked.rowRange(startRow, endRow);
    	it->copyTo(mat);
    }

    return stacked;
}

unsigned long ipa_Utils::EvaluatePolynomial(double x, int degree, double* coefficients, double* y)
{
	(*y) = coefficients[degree];
	for (int i = degree-1; i >= 0; i--)
	{
		(*y) *= x;
		(*y) += coefficients[i];
	}

	return ipa_Utils::RET_OK;
}

unsigned long ipa_Utils::MaskImage(const cv::Mat& source, cv::Mat& dest, const cv::Mat& mask, cv::Mat& destMask, float minMaskThresh, float maxMaskThresh,
						int sourceChannel, double sourceMin, double sourceMax)
{
        double globalMin = -1;
		double globalMax = -1;

		double maskMin = -1;
		double maskMax = -1;

		dest.create(source.rows, source.cols, CV_8UC3);	

		CV_Assert(sourceChannel >= 1);
		CV_Assert(sourceChannel <= source.channels());

		/// Check if destination image has been initialized correctly
		CV_Assert(destMask.depth() == CV_8U);
		CV_Assert(destMask.channels() == 3);
		CV_Assert(destMask.cols == source.cols);
		CV_Assert(destMask.rows == source.rows);

		/// Check if mask image has been initialized correctly
		CV_Assert(mask.depth() == CV_32F);
		CV_Assert(mask.channels() == 1);
		CV_Assert(mask.cols == source.cols);
		CV_Assert(mask.rows == source.rows);

		/// Calculate minmal and maximal value within the specified image sourceChannel
		/// Channel must be within [1, source->nChannels]
		if (sourceMin == -1 || sourceMax == -1)
		{
			cv::Mat mixImage(source.rows, source.cols, source.depth(), 1);

			// Copy channel 2 of source to channel 0 of zSource
			int from_to[] = {sourceChannel-1, 0};

			cv::mixChannels(&source, 1, &mixImage, 1, from_to, 1);
			cv::minMaxLoc(mixImage, &globalMin, &globalMax);
		}
		else
		{
			std::cerr << "ERROR - OpenCVUtils::MaskImage:" << std::endl;
			std::cerr << "\t ... Parameter sourceChannel ('" << sourceChannel << "') out of range.\n";
			return RET_FAILED;
		}
		
		if (sourceMin == -1) sourceMin = globalMin;
		if (sourceMax == -1) sourceMax = globalMax;

		cv::minMaxLoc(mask, &maskMin, &maskMax);
		double wMask = maskMax-maskMin;

		double w = sourceMax-sourceMin;
		int destIndex = 0;
		int sourceIndex = 0;
		int maskIndex = 0;

		if (source.depth() == CV_32F)
		{
			for(int j=0; j<source.rows; j++)
			{
				const float* f_source_ptr = source.ptr<float>(j);
				const float* f_mask_ptr = mask.ptr<float>(j);
				
				unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
				unsigned char* c_destMask_ptr = destMask.ptr<unsigned char>(j);
		
				for(int i=0; i<source.cols; i++)
				{
					unsigned char V = 0;
					unsigned char vMask = 0;
					destIndex = i*3;
					sourceIndex = i*source.channels();
					maskIndex = i*mask.channels();

					double z = (double)f_source_ptr[sourceIndex + sourceChannel - 1];
					float maskVal = f_mask_ptr[maskIndex];
					if (maskVal < maxMaskThresh &&
						maskVal > minMaskThresh)
					{
						if (z < sourceMin) 
						{
							z = sourceMin;
							maskVal = (float)maskMin;
						}
						if (z > sourceMax)
						{
							z = sourceMax;
							maskVal = (float)maskMax;
						}
						V= (unsigned char)(255.0 * ((z-sourceMin)/w));

						vMask= (unsigned char)(255.0 * ((maskVal-globalMin)/wMask));
						
					}
					else
					{
						V = 0;
						vMask = 0;
					}

					c_dest_ptr[destIndex] = V;
					c_dest_ptr[destIndex + 1] = V;
					c_dest_ptr[destIndex + 2] = V;

					c_destMask_ptr[destIndex] = vMask;
					c_destMask_ptr[destIndex + 1] = vMask;
					c_destMask_ptr[destIndex + 2] = vMask;
				}
			}
		}
		else if (source.depth() == CV_32S)
		{
			for(int j=0; j<source.rows; j++)
			{
				const float* f_mask_ptr = mask.ptr<float>(j);
				const int* i_source_ptr = source.ptr<int>(j);
				unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);

				for(int i=0; i<source.cols; i++)
				{
					int iTimes3 = i*3;
					unsigned char V = 0;
		
					double z = (double)i_source_ptr[i*source.channels() + sourceChannel - 1];
				
					float maskVal = f_mask_ptr[maskIndex];
					if (maskVal < maxMaskThresh &&
						maskVal > minMaskThresh)
					{
						if (z < sourceMin) z = sourceMin;
						if (z > sourceMax) z = sourceMax;
						V = (unsigned char)(255.0 * ((z-sourceMin)/w));
					}
					else
					{
						V = 0;
					}

					c_dest_ptr[iTimes3] = V;
					c_dest_ptr[iTimes3 + 1] = V;
					c_dest_ptr[iTimes3 + 2] = V;
				}
			}
		}
		else if (source.depth() == CV_8U)
		{
			for(int j=0; j<source.rows; j++)
			{
				const float* f_mask_ptr = mask.ptr<float>(j);
				const unsigned char* c_source_ptr = source.ptr<unsigned char>(j);
				unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);

				for(int i=0; i<source.cols; i++)
				{
					int iTimes3 = i*3;
					unsigned char V = 0;
		
					double z = (double)c_source_ptr[i*source.channels() + sourceChannel - 1];
	
					float maskVal = f_mask_ptr[maskIndex];
					if (maskVal < maxMaskThresh &&
						maskVal > minMaskThresh)
					{
						if (z < sourceMin) z = sourceMin;
						if (z > sourceMax) z = sourceMax;
						V = (unsigned char)(255.0 * ((z-sourceMin)/w));
					}
					else
					{
						V = 0;
					}

					c_dest_ptr[iTimes3] = V;
					c_dest_ptr[iTimes3 + 1] = V;
					c_dest_ptr[iTimes3 + 2] = V;
				}
			}
		}
		else
		{
			std::cout << "ERROR - OpenCVUtils::MaskImage:" << std::endl;
			std::cout << "\t ... Image depth of source not supported.\n";
			return RET_FAILED;
		}
		

		return RET_OK;
}

unsigned long ipa_Utils::ConvertToShowImage(const cv::Mat& source, cv::Mat& dest, int channel, double min, double max)
{
        double globalMin = -1;
		double globalMax = -1;

		CV_Assert(channel >= 1);
		CV_Assert(channel <= source.channels());

		dest.create(source.rows, source.cols, CV_8UC3);	

		/// Calculate minmal and maximal value within the specified image channel
		cv::Mat mixImage(source.rows, source.cols, source.depth(), 1);

		// Copy channel 2 of source to channel 0 of zSource
		int from_to[] = {channel-1, 0};

		cv::mixChannels(&source, 1, &mixImage, 1, from_to, 1);
		cv::minMaxLoc(mixImage, &globalMin, &globalMax);
				
		if (min == -1) min = globalMin;
		if (max == -1) max = globalMax;

		double w = max-min;

		if (source.depth() == CV_32F)
		{
			for(int j=0; j<source.rows; j++)
			{
				const float* f_source_ptr = source.ptr<float>(j);				
				unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);

				for(int i=0; i<source.cols; i++)
				{
					int iTimes3 = i*3;
		
					double z = (double)f_source_ptr[i*source.channels() + channel - 1];
					
					if (z < min) z = min;
					if (z > max) z = max;

					int V= (int)(255.0 * ((z-min)/w));

					c_dest_ptr[iTimes3] = V;
					c_dest_ptr[iTimes3 + 1] = V;
					c_dest_ptr[iTimes3 + 2] = V;
				}
			}
		}
		else if (source.depth() == CV_32S)
		{
			for(int j=0; j<source.rows; j++)
			{
				const int* i_source_ptr = source.ptr<int>(j);
				unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);

				for(int i=0; i<source.cols; i++)
				{
					int iTimes3 = i*3;
		
					double z = (double)i_source_ptr[i*source.channels() + (channel - 1)];
				
					if (z < min) z = min;
					if (z > max) z = max;		

					int V= (int)(255.0 * ((z-min)/w));

					c_dest_ptr[iTimes3] = V;
					c_dest_ptr[iTimes3 + 1] = V;
					c_dest_ptr[iTimes3 + 2] = V;
				}
			}
		}
		else if (source.depth() == CV_8U)
		{
			for(int j=0; j<source.rows; j++)
			{
				const unsigned char* c_source_ptr = source.ptr<unsigned char>(j);
				unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);

				for(int i=0; i<source.cols; i++)
				{
					int iTimes3 = i*3;
		
					double z = (double)c_source_ptr[i*source.channels() + (channel - 1)];
	
					if (z < min) z = min;
					if (z > max) z = max;

					int V= (int)(255.0 * ((z-min)/w));

					c_dest_ptr[iTimes3] = V;
					c_dest_ptr[iTimes3 + 1] = V;
					c_dest_ptr[iTimes3 + 2] = V;
				}
			}
		}
		else if (source.depth() == CV_16U)
		{
			for(int j=0; j<source.rows; j++)
			{
				const unsigned short* c_source_ptr = source.ptr<unsigned short>(j);
				unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);

				for(int i=0; i<source.cols; i++)
				{
					int iTimes3 = i*3;
		
					double z = (double)c_source_ptr[i*source.channels() + (channel - 1)];
	
					if (z < min) z = min;
					if (z > max) z = max;

					int V= (int)(255.0 * ((z-min)/w));

					c_dest_ptr[iTimes3] = V;
					c_dest_ptr[iTimes3 + 1] = V;
					c_dest_ptr[iTimes3 + 2] = V;
				}
			}
		}
		else
		{
			std::cout << "ERROR - OpenCVUtils::ConvertToShowImage:" << std::endl;
			std::cout << "\t ... Image depth of source not supported.\n";
			return RET_FAILED;
		}
		

		return RET_OK;
}

unsigned long ipa_Utils::FilterByAmplitude(cv::Mat& xyzImage, const cv::Mat& greyImage, cv::Mat* mask, cv::Mat* maskColor, float minMaskThresh, float maxMaskThresh)
{
	CV_Assert(xyzImage.type() == CV_32FC3);
	CV_Assert(greyImage.type() == CV_32FC1);

	if(mask) mask->create(greyImage.size(), CV_32FC1);
	if(maskColor) maskColor->create(greyImage.size(), CV_8UC3);

	int xyzIndex = 0;
	int maskColorIndex = 0;
	float V = 0;
	float vMask = 0;

	unsigned char* c_maskColor_ptr = 0;
	float* f_xyz_ptr = 0;
	const float* f_grey_ptr = 0;
	float* f_mask_ptr = 0;

	for(int j=0; j<xyzImage.rows; j++)
	{
		f_xyz_ptr = xyzImage.ptr<float>(j);
		f_grey_ptr = greyImage.ptr<float>(j);
		if(mask) f_mask_ptr = mask->ptr<float>(j);
		if(maskColor) c_maskColor_ptr = maskColor->ptr<unsigned char>(j);

		for(int i=0; i<xyzImage.cols; i++)
		{
			
			xyzIndex = i*3;
			maskColorIndex = i*3;

			double z = (double)f_xyz_ptr[xyzIndex + 2];
			float maskVal = f_grey_ptr[i];

			if(maskColor)
			{
				/// build color mask from amplitude values
				if(maskVal>maxMaskThresh)
				{
					c_maskColor_ptr[maskColorIndex]= 0;
					c_maskColor_ptr[maskColorIndex+1]= 0;
					c_maskColor_ptr[maskColorIndex+2]= 255;
				}
				else if(maskVal<minMaskThresh)
				{
					c_maskColor_ptr[maskColorIndex]= 0;
					c_maskColor_ptr[maskColorIndex+1]= 255;
					c_maskColor_ptr[maskColorIndex+2]= 0;
				}
				else if(z<0.3)
				{
					c_maskColor_ptr[maskColorIndex]= 255;
					c_maskColor_ptr[maskColorIndex+1]= 0;
					c_maskColor_ptr[maskColorIndex+2]= 0;
				}
				else
				{
					c_maskColor_ptr[maskColorIndex]= 0;
					c_maskColor_ptr[maskColorIndex+1]= 0;
					c_maskColor_ptr[maskColorIndex+2]= 0;
				}
			}

			if (maskVal < maxMaskThresh &&
				maskVal > minMaskThresh)
			{
				vMask = 0;
			}
			else
			{
				vMask = 1;
				f_xyz_ptr[xyzIndex] = V;
				f_xyz_ptr[xyzIndex + 1] = V;
				f_xyz_ptr[xyzIndex + 2] = V;
			}

			if(mask)
			{
				f_mask_ptr[i] = vMask;
			}

		}
	}

	return RET_OK;
}

unsigned long ipa_Utils::FilterTearOffEdges(cv::Mat& xyzImage, cv::Mat* mask, float piHalfFraction)
{
	/// Check if destination image has been initialized correctly
	CV_Assert(xyzImage.type() == CV_32FC3);

	float pi = 3.14159f;
	float t_lower =pi/piHalfFraction;
	float t_upper = pi - t_lower;

	if(mask)
	{
		mask->create(xyzImage.size() , CV_8UC3);
		mask->setTo(0);
	}

	for(int row=0; row < xyzImage.rows; row++)
	{
		int index_vLeft = -1;
		int index_vMiddle = -1;
		int index_vRight = -1;
		int index_vUp = -1;
		int index_vDown = -1;

		cv::Vec3f vLeft = cv::Vec3f::all(0);
		cv::Vec3f vMiddle = cv::Vec3f::all(0);
		cv::Vec3f vRight = cv::Vec3f::all(0);
		cv::Vec3f vUp = cv::Vec3f::all(0);
		cv::Vec3f vDown = cv::Vec3f::all(0);

		cv::Vec3f vDiff = cv::Vec3f::all(0);

		float* f_image_ptr_RowUp = 0;
		float* f_image_ptr_RowMiddle = 0;
		float* f_image_ptr_RowDown = 0;

		float dot = -1.f;
		float angle = -1.f;

		if (row-1 >= 0)
		{
			f_image_ptr_RowUp = xyzImage.ptr<float>(row-1);
		}

		f_image_ptr_RowMiddle = xyzImage.ptr<float>(row);

		if (row+1 < xyzImage.rows)
		{
			f_image_ptr_RowDown = xyzImage.ptr<float>(row+1);
		}

		/// Extract four surrounding neighbor vectors that have a non zero mask value
		///
		///    x
		///  x o x
		///    x
		///
		for(int col=0; col < xyzImage.cols; col++)
		{
			/// Counte the amount of times, we satisfy the thresholds
			int score = 0;

			/// Vector Middle (must exist)
			index_vMiddle = col;
			vMiddle[0] = f_image_ptr_RowMiddle[3*index_vMiddle];
			vMiddle[1] = f_image_ptr_RowMiddle[3*index_vMiddle + 1];
			vMiddle[2] = f_image_ptr_RowMiddle[3*index_vMiddle + 2];

			/// Vector Left
			if (col-1 >= 0)
			{
				index_vLeft = col-1;
				vLeft[0] = f_image_ptr_RowMiddle[3*index_vLeft];
				vLeft[1] = f_image_ptr_RowMiddle[3*index_vLeft + 1];
				vLeft[2] = f_image_ptr_RowMiddle[3*index_vLeft + 2];
				vDiff = vLeft - vMiddle;
				float vLeftNorm = std::sqrt((vLeft[0] * vLeft[0]) + 
					(vLeft[1] * vLeft[1]) + (vLeft[2] * vLeft[2]));
				vLeft[0] = vLeft[0]/vLeftNorm;
				vLeft[1] = vLeft[1]/vLeftNorm;
				vLeft[2] = vLeft[2]/vLeftNorm;
				//vLeft.Normalize();
				float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
					(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
				vDiff[0] = vDiff[0]/vDiffNorm;
				vDiff[1] = vDiff[1]/vDiffNorm;
				vDiff[2] = vDiff[2]/vDiffNorm;
				//vDiff.Normalize();
				dot = (float)vDiff.ddot(vLeft);
				//dot = vDiff.Dot(vLeft);
				angle = (float)std::acos(dot);
				//angle = IPA_WM_VERSION::Math<float>::ACos( dot );
				if (angle > t_upper || angle < t_lower)
				{
					score++;
				}
				else
				{
					score--;
				}
			}

			/// Vector Right
			if (col+1 < xyzImage.rows)
			{
				index_vRight = col+1;
				vRight[0] = f_image_ptr_RowMiddle[3*index_vRight];
				vRight[1] = f_image_ptr_RowMiddle[3*index_vRight + 1];
				vRight[2] = f_image_ptr_RowMiddle[3*index_vRight + 2];
				vDiff = vRight - vMiddle;
				float vRightNorm = std::sqrt((vRight[0] * vRight[0]) + 
					(vRight[1] * vRight[1]) + (vRight[2] * vRight[2]));
				vRight[0] = vRight[0]/vRightNorm;
				vRight[1] = vRight[1]/vRightNorm;
				vRight[2] = vRight[2]/vRightNorm;
				//vRight.Normalize();
				float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
					(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
				vDiff[0] = vDiff[0]/vDiffNorm;
				vDiff[1] = vDiff[1]/vDiffNorm;
				vDiff[2] = vDiff[2]/vDiffNorm;
				//vDiff.Normalize();
				dot = (float)vDiff.ddot(vLeft);
				//dot = vDiff.Dot(vLeft);
				angle = (float)std::acos(dot);
				//angle = IPA_WM_VERSION::Math<float>::ACos( dot );
				if (angle > t_upper || angle < t_lower)
				{
					score++;
				}
				else
				{
					score--;
				}
			}

			/// Vector Up
			if (f_image_ptr_RowUp)
			{
				index_vUp = col;
				vUp[0] = f_image_ptr_RowUp[3*index_vUp];
				vUp[1] = f_image_ptr_RowUp[3*index_vUp + 1];
				vUp[2] = f_image_ptr_RowUp[3*index_vUp + 2];
				vDiff = vUp - vMiddle;
				float vUpNorm = std::sqrt((vUp[0] * vUp[0]) + 
					(vUp[1] * vUp[1]) + (vUp[2] * vUp[2]));
				vUp[0] = vUp[0]/vUpNorm;
				vUp[1] = vUp[1]/vUpNorm;
				vUp[2] = vUp[2]/vUpNorm;
				//vUp.Normalize();
				float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
					(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
				vDiff[0] = vDiff[0]/vDiffNorm;
				vDiff[1] = vDiff[1]/vDiffNorm;
				vDiff[2] = vDiff[2]/vDiffNorm;
				//vDiff.Normalize();
				dot = (float)vDiff.ddot(vLeft);
				//dot = vDiff.Dot(vLeft);
				angle = (float)std::acos(dot);
				//angle = IPA_WM_VERSION::Math<float>::ACos( dot );
				if (angle > t_upper || angle < t_lower)
				{
					score++;
				}
				else
				{
					score--;
				}
			}

			/// Vector Down
			if (f_image_ptr_RowDown)
			{
				index_vDown = col;
				vDown[0] = f_image_ptr_RowDown[3*index_vDown];
				vDown[1] = f_image_ptr_RowDown[3*index_vDown + 1];
				vDown[2] = f_image_ptr_RowDown[3*index_vDown + 2];
				float vDownNorm = std::sqrt((vDown[0] * vDown[0]) + 
					(vDown[1] * vDown[1]) + (vDown[2] * vDown[2]));
				vDown[0] = vDown[0]/vDownNorm;
				vDown[1] = vDown[1]/vDownNorm;
				vDown[2] = vDown[2]/vDownNorm;
				//vDown.Normalize();
				float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
					(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
				vDiff[0] = vDiff[0]/vDiffNorm;
				vDiff[1] = vDiff[1]/vDiffNorm;
				vDiff[2] = vDiff[2]/vDiffNorm;
				//vDiff.Normalize();
				dot = (float)vDiff.ddot(vLeft);
				//dot = vDiff.Dot(vLeft);
				angle = (float)std::acos(dot);
				//angle = IPA_WM_VERSION::Math<float>::ACos( dot );
				if (angle > t_upper || angle < t_lower)
				{
					score++;
				}
				else
				{
					score--;
				}
			}


			/// Mask value if angle exceeded threshold too often
			if (score > 0)
			{
				cv::Vec3b pt(0, 0, 0);
				if(mask)
				{
					mask->at<cv::Vec3b>(row,col)=pt;
				}
//				xyzImage.at<cv::Vec3f>(row, col) = pt;
				for(int i = 0; i < 3; i++)
					((float*)xyzImage.ptr(row))[3*col+i] = 0.f;
			}
		}
	}

	return ipa_Utils::RET_OK;
}

unsigned long ipa_Utils::FilterSpeckles(cv::Mat& img, int maxSpeckleSize, 
	double maxDiff, cv::Mat& _buf)
{
    CV_Assert( img.type() == CV_32FC3 );

    float newVal = 0;
    int width = img.cols, height = img.rows, npixels = width*height;
    size_t bufSize = npixels*(int)(sizeof(cv::Point_<short>) + sizeof(int) + sizeof(uchar));
    if( !_buf.isContinuous() || !_buf.data || _buf.cols*_buf.rows*_buf.elemSize() < bufSize )
        _buf.create(1, bufSize, CV_8U);
    
    uchar* buf = _buf.data;
    int i, j, dstep = img.step/sizeof(cv::Vec3f);
    int* labels = (int*)buf;
    buf += npixels*sizeof(labels[0]);
    cv::Point_<short>* wbuf = (cv::Point_<short>*)buf;
    buf += npixels*sizeof(wbuf[0]);
    uchar* rtype = (uchar*)buf;
    int curlabel = 0;
    
    // clear out label assignments
    memset(labels, 0, npixels*sizeof(labels[0]));
    
    for( i = 0; i < height; i++ )
    {
		cv::Vec3f* ds = img.ptr<cv::Vec3f>(i);
        int* ls = labels + width*i;
        
        for( j = 0; j < width; j++ )
        {
            if( ds[j][2] != newVal )	// not a bad disparity
            {
                if( ls[j] )		// has a label, check for bad label
                {  
                    if( rtype[ls[j]] ) // small region, zero out disparity
					{
                        ds[j][0] = (float)newVal;
						ds[j][1] = (float)newVal;
						ds[j][2] = (float)newVal;
					}
                }
                // no label, assign and propagate
                else
                {
                    cv::Point_<short>* ws = wbuf;	// initialize wavefront
                    cv::Point_<short> p((short)j, (short)i);	// current pixel
                    curlabel++;	// next label
                    int count = 0;	// current region size
                    ls[j] = curlabel;
                    
                    // wavefront propagation
                    while( ws >= wbuf ) // wavefront not empty
                    {
                        count++;
                        // put neighbors onto wavefront
						cv::Vec3f* dpp = &img.at<cv::Vec3f>(p.y, p.x);
						cv::Vec3f dp = *dpp;
                        int* lpp = labels + width*p.y + p.x;
                        
                        if( p.x < width-1 && !lpp[+1] && dpp[+1][2] != newVal && std::abs(dp[2] - dpp[+1][2]) <= maxDiff )
                        {
                            lpp[+1] = curlabel;
                            *ws++ = cv::Point_<short>(p.x+1, p.y);
                        }
                        
						if( p.x > 0 && !lpp[-1] && dpp[-1][2] != newVal && std::abs(dp[2] - dpp[-1][2]) <= maxDiff )
                        {
                            lpp[-1] = curlabel;
                            *ws++ = cv::Point_<short>(p.x-1, p.y);
                        }
                        
                        if( p.y < height-1 && !lpp[+width] && dpp[+dstep][2] != newVal && std::abs(dp[2] - dpp[+dstep][2]) <= maxDiff )
                        {
                            lpp[+width] = curlabel;
                            *ws++ = cv::Point_<short>(p.x, p.y+1);
                        }
                        
                        if( p.y > 0 && !lpp[-width] && dpp[-dstep][2] != newVal && std::abs(dp[2] - dpp[-dstep][2]) <= maxDiff )
                        {
                            lpp[-width] = curlabel;
                            *ws++ = cv::Point_<short>(p.x, p.y-1);
                        }
                        
                        // pop most recent and propagate
                        // NB: could try least recent, maybe better convergence
                        p = *--ws;
                    }
                    
                    // assign label type
                    if( count <= maxSpeckleSize )	// speckle region
                    {
                        rtype[ls[j]] = 1;	// small region label
                        ds[j][0] = (float)newVal;
						ds[j][1] = (float)newVal;
						ds[j][2] = (float)newVal;
                    }
                    else
                        rtype[ls[j]] = 0;	// large region label
                }
            }
        }
    }
	return ipa_Utils::RET_OK;
} 
   
cv::Vec3b ipa_Utils::GrayColorMap(double value, double min,double max)
{
    double rgb[3];
    max-=min;
    value-=min;
    rgb[0]=rgb[1]=rgb[2]=(unsigned char)(255*value/max);
    return cv::Vec3b(rgb[2], rgb[1], rgb[0]);
}

unsigned long ipa_Utils::SaveMat(cv::Mat& mat, std::string filename, int type)
{
	

	std::ofstream f(filename.c_str(), std::ios_base::binary);
	if(!f.is_open())
	{
		std::cerr << "ERROR - ipa_Utils::SaveMat:" << std::endl;
		std::cerr << "\t ... Could not open " << filename << " \n";
		return ipa_Utils::RET_FAILED;
	}

	int channels = mat.channels();
	
	int header[3];
	header[0] = mat.rows;
	header[1] = mat.cols;
	header[2] = channels;

#ifndef __LINUX__
	f.write((char*)header, 3 * sizeof(int));
#else
	f.write((char const*)header, 3 * sizeof(int));
#endif

	if (type == CV_32F)
	{
		float* ptr = 0;
		for(unsigned int row=0; row<(unsigned int)mat.rows; row++)
		{
			ptr = mat.ptr<float>(row);
			f.write((char*)ptr, channels * mat.cols * sizeof(float));
		}
	}
	if (type == CV_8U)
	{
		unsigned char* ptr = 0;
		for(unsigned int row=0; row<(unsigned int)mat.rows; row++)
		{
			ptr = mat.ptr<unsigned char>(row);
			f.write((char*)ptr, channels * mat.cols * sizeof(unsigned char));
		}
	}

	f.close();
	return ipa_Utils::RET_OK;
}

unsigned long ipa_Utils::LoadMat(cv::Mat& mat, std::string filename, int type)
{
	size_t file_length = 0;
	char *c_string = 0;

	std::ifstream file(filename.c_str(), std::ios_base::binary|std::ios_base::in|std::ios_base::ate);
	if(!file.is_open())
	{
		std::cerr << "ERROR - ipa_Utils::LoadMat:" << std::endl;
		std::cerr << "\t ... Could not open " << filename << " \n";
		return ipa_Utils::RET_FAILED;
	}

	file_length = file.tellg();
	file.seekg(0, std::ios_base::beg);
	file.clear();

	c_string = new char[file_length];
	file.read(c_string, file_length);
	
	unsigned int rows, cols;
	int channels;
	rows = ((int*)c_string)[0];
	cols = ((int*)c_string)[1];
	channels = ((int*)c_string)[2];

	char* c_ptr;

	if (type == CV_32F)
	{
		float* f_ptr;
		mat.create(rows, cols, CV_32FC(channels));
		f_ptr = mat.ptr<float>(0);
		c_ptr = &c_string[3 * sizeof(int)];
	
		memcpy(f_ptr, c_ptr,  channels * mat.cols * mat.rows * sizeof(float));
	}
	if (type == CV_8U)
	{
		unsigned char* f_ptr;
		mat.create(rows, cols, CV_32FC(channels));
		f_ptr = mat.ptr<unsigned char>(0);
		c_ptr = &c_string[3 * sizeof(int)];
	
		memcpy(f_ptr, c_ptr,  channels * mat.cols * mat.rows * sizeof(unsigned char));
	}

	file.close();

	delete[] c_string;

	return ipa_Utils::RET_OK;
}

ipa_Utils::UniqueNumber::UniqueNumber()
{
    current=0;
}

int ipa_Utils::UniqueNumber::operator()() 
{
    return current++;
}
